/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:11:42
 * @LastEditTime: 2021-10-26 21:09:10
 * @LastEditors: Sang Hao
 */

#include "glog/logging.h"
#include "lidar_slam/subscriber/key_frames_subscriber.hpp"

namespace lidar_slam {
/**
 * @description: 构造函数
 * @param  {*}
 * @return {*}
 * @param {string} topic_name
 * @param {size_t} buff_size
 */
KeyFramesSubscriber::KeyFramesSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 
	: nh_(nh) {
	subscriber_ = nh_.subscribe(topic_name, buff_size, &KeyFramesSubscriber::msg_callback, this);
}
/**
 * @description: subscriber订阅的函数类型为消息类型+ConstPtr.回调函数，导入到new_key_frames_中
 * @param  {*}
 * @return {*}
 */
void KeyFramesSubscriber::msg_callback(const nav_msgs::Path::ConstPtr& key_frames_msg_ptr) {
	new_key_frames_.clear();
	/* for循环就是和KeyFrameSubscriber的区别 */
	for (size_t i = 0; i < key_frames_msg_ptr->poses.size(); ++i) {
		KeyFrame key_frame;
		key_frame.time = key_frames_msg_ptr->poses.at(i).header.stamp.toSec();
		key_frame.index = (unsigned int)i;

		key_frame.pose(0, 3) = key_frames_msg_ptr->poses.at(i).pose.position.x;
		key_frame.pose(1, 3) = key_frames_msg_ptr->poses.at(i).pose.position.y;
		key_frame.pose(2, 3) = key_frames_msg_ptr->poses.at(i).pose.position.z;

		Eigen::Quaternionf q;
		
		q.x() = key_frames_msg_ptr->poses.at(i).pose.orientation.x;
		q.y() = key_frames_msg_ptr->poses.at(i).pose.orientation.y;
		q.z() = key_frames_msg_ptr->poses.at(i).pose.orientation.z;
		q.w() = key_frames_msg_ptr->poses.at(i).pose.orientation.w;
		key_frame.pose.block<3, 3>(0, 0) = q.matrix();

		new_key_frames_.push_back(key_frame);
	}
}
/**
 * @description: 解析数据，将new_key_frames_替代为key_frames_buff
 * @param  {*}
 * @return {*}
 */
void KeyFramesSubscriber::ParseData(std::deque<KeyFrame>& key_frames_buff) {
	if (new_key_frames_.size() > 0) {
		/* 这里不用insert，因为本就是frames */
		key_frames_buff = new_key_frames_;
		new_key_frames_.clear();
	}
}
}